/*
 * Copyright 2010 The MIauto Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#define F_CPU 7372800UL /* 7.3728 MHz External Oscillator */

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include <util/delay.h>
#include <avr/pgmspace.h>
#include <stdint.h>
#include "servo.h"
#include "xbee.h"

#define MIAUTO_SWITCH_ON 0xff
#define MIAUTO_SWITCH_OFF 0x00

#define MIAUTO_PROTOCOL_DEFINITION_BYTE 0

uint8_t miAutoState = 0x00;

enum
{
	SERVOA = 'a',
	SERVOB = 'b',
	STARTSTOP = 'c',
	SET_DIR_SETTINGS = 'd',
	SET_GAS_SETTINGS = 'e',
	WATCHDOG_RESET = 'f',
};

enum miAutoStates
{
	miauto_idle = 0x00, miauto_running,
};

int main(void)
{
	t_Servo tServo;
	uint64_t src;
	uint8_t sigstrength;
	uint8_t* data;
	uint8_t datalength;

	/* Init PWM */
	PWM_Init();

	/* Init USART */
	USART_Init();

	sei();

	PacketData *receivedPacket = XBee_GetReceivePacket();
	XBee_ResetPacket(receivedPacket);

	for (;;)
	{
		/* todo remove delay without errors on device */
		_delay_ms(1);

		/* if a packet has been received */
		if (receivedPacket->finished == TRUE)
		{
			switch (receivedPacket->apiId)
			{

			/* If the packet is a RX64 Packet */
			case (XBEE_RX64):
			{
				if (XBee_ReadRX64Packet(receivedPacket, &src, &sigstrength,
						NULL, &data, &datalength))
				{
					/* Package handling depends on the state of the MIauto */
					switch (miAutoState)
					{

					/* The MIauto is running */
					case miauto_running:
						switch (data[0])
						{
						case SERVOA:
							setDirect(&servos[DIRECTION_SERVO], data[1]);
							break;
						case SERVOB:
							setDirect(&servos[THROTLE_SERVO], data[1]);

							break;
						case STARTSTOP:

							if (data[1] == MIAUTO_SWITCH_OFF)
							{
								stopServos();
								miAutoState = miauto_idle;
								wdt_disable();
							}
							break;
						case WATCHDOG_RESET:
							wdt_reset();
							break;
						default:
							break;
						}
						break;
					case miauto_idle:

						switch (data[0])
						{
						case STARTSTOP:
							if (data[1] == MIAUTO_SWITCH_ON)
							{
								stopServos();
								miAutoState = miauto_running;
								wdt_enable(WDTO_500MS);
							}
							break;
						case SET_DIR_SETTINGS:
							memcpy(&tServo, &data[1], T_SERVO_STRUCT_SIZE);

							setServoSettings(&servos[DIRECTION_SERVO],
									tServo.u8Multiplyer,
									tServo.u16DefaultDirection,
									tServo.u16MaxDirection,
									tServo.u16MinDirection,
									tServo.u16Direction, &OCR1A);

							saveServoSettings(&servos[DIRECTION_SERVO]);

							break;
						case SET_GAS_SETTINGS:
							memcpy(&tServo, &data[1], T_SERVO_STRUCT_SIZE);

							setServoSettings(&servos[THROTLE_SERVO],
									tServo.u8Multiplyer,
									tServo.u16DefaultDirection,
									tServo.u16MaxDirection,
									tServo.u16MinDirection,
									tServo.u16Direction, &OCR1B);
							saveServoSettings(&servos[DIRECTION_SERVO]);

							break;
						default:
							break;
						}

						break;
					}
				}

			}
				break;
			}

			/* Important! Reset the packet state and starts the USART receive interrupt. */
			XBee_ResetPacket(receivedPacket);
		}
	}
}

